#pragma once

#ifndef _FEATURE_EXTRACT_H_
#define _FEATURE_EXTRACT_H_

#include "common_include.h"
#include"config.h"
#include"utility.h"

/**
 * Velodyne点云结构，变量名XYZIRT是每个变量的首字母
*/
struct VelodynePointXYZIRT
{
    PCL_ADD_POINT4D     // 位置
    PCL_ADD_INTENSITY;  // 激光点反射强度，也可以存点的索引
    uint16_t ring;      // 扫描线
    float time;         // 时间戳，记录相对于当前帧第一个激光点的时差，第一个点time=0
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;        // 内存16字节对齐，EIGEN SSE优化要求
// 注册为PCL点云格式
POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT,
    (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
    (uint16_t, ring, ring) (float, time, time)
)

namespace vld_imu_slam{

class FeatureExtract{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef shared_ptr<FeatureExtract> Ptr;

    pcl::PointCloud<PointType>::Ptr pc_surface_;
    pcl::PointCloud<PointType>::Ptr pc_conner_;
    pcl::PointCloud<PointType>::Ptr pc_full_;
    // pcl::PointCloud<PointType>::Ptr pc_groundPt_sample_;
    // pcl::PointCloud<PointType>::Ptr pc_cornerPt_sample_;
        // pcl::PointCloud<PointType>::Ptr pc_ground_;

    double time_interval_VLD16_s_ = 3472*10^(-9);

    enum EPointType
    {
        e_pt_normal_ = 0,                      // normal points
        e_pt_return_ = 0x0001 << 1,  
        e_pt_reflectivity_low_ = 0x0001 << 2,  // low reflectivity
        e_pt_reflectivity_high_ = 0x0001 << 3, // high reflectivity
        e_pt_nan_ = 0x0001 << 5               // points with infinite value
    };

    enum EPointLabel
    {
        e_uncertain_ =0,
        e_plane_ =0x0001 << 1 ,  
        e_corner_ = 0x0001 << 2,
        e_corner_neighbour = 0x0001 << 3
    };

    struct PointInfo
    {
        double time_stamp_ = -1;
        int line = -1;
        double x_ = -1;
        double y_ = 0;
        double z_ = 0;
        int point_type_  = EPointType::e_pt_nan_;
        int point_label_ = EPointLabel::e_uncertain_;
        double range_ = -1;
    };

    struct PointIdx{
        int i =0;
        int j = 0;
    };

    struct RoughNessIdx{
        PointIdx idx;
        double roughness = 0;
    };



    FeatureExtract(){}
    FeatureExtract(vld_imu_slam::Config::Ptr p_config);
    
    void pcHandle(const sensor_msgs::PointCloud2ConstPtr &msg);

private:
    pcl::PointCloud<VelodynePointXYZIRT>::Ptr pc_raw_;
    vld_imu_slam::Config::Ptr p_config_;

    double pc_last_timestamp_ = 0;

    vector<vector<PointInfo>> pc_array_;
    vector<vector<RoughNessIdx>> roughness_array_; 

    void judgeGround();

    void featureExtract();

    void judgeRoughness(PointIdx& p_idx);

    void judgePoint(PointInfo& pt, Eigen::Vector3d& r_v,int& num);

    void projectPC(const sensor_msgs::PointCloud2ConstPtr &msg);

    void samplePoint();

    // bool addGroundPoint(PointInfo& pt);

    void addCornerPoint(PointInfo& pt,pcl::PointCloud<PointType>::Ptr pc);

    void addPlanePoint(PointInfo& pt,pcl::PointCloud<PointType>::Ptr pc);

    void markOccludedPoints();

    struct by_value{ 
    bool operator()(RoughNessIdx const &left, RoughNessIdx const &right) { 
        return left.roughness < right.roughness;
    }

};

};



}
#endif  
